#define DEBUG         0 
#define FP_PRE        0.0000001
#define USAGE_MESSAGE "Usage: llssphere <file>"
#define DATA_POINTS   4
#define EVIDENCE_TRSH 1000

#include <iostream>
using std::cout;
using std::cerr;
using std::endl;

#include <stdio.h>
#include <stdlib.h>
#include <cstdlib>
#include <string.h>
#include <time.h>
#include <math.h>
#include <cmath>
#include <fstream>
using std::ifstream;
using std::ofstream;

#include "util.h"

typedef struct {
	double x;
	double y;
	double z;
} edge_pixel_t, point_3d_t;

typedef struct {
	matrix_t center_x_y;
	matrix_t center_z_radius;
} accumulator_t;

typedef struct {
	double x;
	double y;
	double z;
	double radius;
} sphere_t;

typedef struct {
	point_3d_t p1;
	point_3d_t p2;
	point_3d_t p3;
	point_3d_t p4;
} bezier_t;

string        in_filename        ;
string        out_filename       ;
edge_pixel_t* edge_pxls    = NULL;
int           edge_pxl_cnt = 0   ;
int			  width        = 500 ;
int			  height       = 500 ;
int			  depth        = 500 ;
accumulator_t accum        = {{0, 0, NULL}, {0, 0, NULL}};

void setup(int*, char**);
void setup_accum();

void terminate();

void shutdown_accum();
void gather_evidence();
edge_pixel_t* random_points(int);
void fit_points_4d(int, int*, double &, double &, double &, double &);
void process_accum(sphere_t*);
void find_max_2d(matrix_t*, int &, int &);

int main (int argc, char *argv[]) {
	sphere_t sphere;
	timespec start, end, t_ge, t_pa;

	// setup the program
	setup(&argc,argv);

	// gather evidence (timed)
	clock_gettime(CLOCK_PROCESS_CPUTIME_ID,&start);
	gather_evidence();
	clock_gettime(CLOCK_PROCESS_CPUTIME_ID,&end);
	time_diff(&start,&end,&t_ge);
	//print_time(&diff,"gather_evidence");

	// recover the circle from the evidence
	clock_gettime(CLOCK_PROCESS_CPUTIME_ID,&start);
	process_accum(&sphere);
	clock_gettime(CLOCK_PROCESS_CPUTIME_ID,&end);
	time_diff(&start,&end,&t_pa);
	
	cout << "x.center: " << sphere.x << endl;
	cout << "y.center: " << sphere.y << endl;
	cout << "z.center: " << sphere.z << endl;
	cout << "radius:   " << sphere.radius << endl;

	out_filename = in_filename + "-output";
	
	int numPixels = 0, numEdgePixels = 360*180;
	edge_pixel_t * edgePixels = new edge_pixel_t[numEdgePixels];

	FILE* file;

	file = fopen(out_filename.c_str(), "w");
	fprintf(file, "4\n");
	fprintf(file,"%f %f %f %f\n", sphere.x, sphere.y, sphere.z, sphere.radius);

	for(double azimuth = 0.0; azimuth < 180.0; azimuth += 0.5)
	{
			for(double polar = 0.0; polar < 90.0; polar += 0.5)
			{
					edge_pixel_t tempPt;
					tempPt.x = sphere.x + sphere.radius * sin(azimuth) * cos(polar);
					tempPt.y = sphere.y + sphere.radius * sin(azimuth) * sin(polar);
					tempPt.z = sphere.z + sphere.radius * cos(azimuth);
					
					edgePixels[numPixels] = tempPt;
					
					numPixels++;
					

					//fprintf(file,"%f %f %f\n", x,y,z);
			}
	}
	
	fprintf(file,"%d\n", numPixels);
	
	for(int i = 0; i < numEdgePixels; i++)
	{
		fprintf(file,"%f %f %f\n", edgePixels[i].x,edgePixels[i].y,edgePixels[i].z);
	}

	fclose(file);
	
	
	terminate();
	return 0;
}

void setup(int* argc, char **argv) {
	int pos;

	// check the lenght of the cmd line
	if (*argc != 2)
		cerr << USAGE_MESSAGE << endl;

	// seed the rng
	srand(clock());

	// load the input image
	debug("Reading in file %s...",argv[1]);
	in_filename = argv[1];
	
	ifstream inFile(in_filename.c_str());
	double temp;
	
	inFile >> temp >> temp >> temp >> temp >> temp >> edge_pxl_cnt;
	
	//cout << edge_pxl_cnt << endl;
	
	edge_pxls = new edge_pixel_t[edge_pxl_cnt];
	memset(edge_pxls,0,sizeof(edge_pixel_t) * edge_pxl_cnt);
	
	for(int i = 0; i < edge_pxl_cnt; i++)
	{
		inFile >> temp;
		edge_pxls[i].x = temp;

		inFile >> temp;
		edge_pxls[i].y = temp;

		inFile >> temp;
		edge_pxls[i].z = temp;
	}

	// setup the accumulator
	setup_accum();

}

void setup_accum() {
	// allocate the accumulator for the ellipse center
	matrix_allocate(&accum.center_x_y,width,height);
	matrix_allocate(&accum.center_z_radius, depth, depth/2);
}

void terminate() {
	if (edge_pxls != NULL) {
		delete [] edge_pxls;
		edge_pxls    = NULL;
		edge_pxl_cnt = 0;
	}

	shutdown_accum();
}

void shutdown_accum() {
	matrix_teardown(accum.center_x_y);
	matrix_teardown(accum.center_z_radius);
}

void gather_evidence() {
	int evidence, *pxls;
	double M_11, M_12, M_13, M_14, M_15;
	sphere_t sphere;

	// allocate the pixels storage
	pxls = new int[DATA_POINTS];

	evidence = 0;
	while (evidence < EVIDENCE_TRSH) {
		memset(&sphere,0,sizeof(sphere_t));

		// randomly select DATA_POINTS # of edge pixels
		for (int i=0; i < DATA_POINTS; i++)
		{
			pxls[i] = random_int(0,edge_pxl_cnt);
		}
		
		edge_pixel_t A = edge_pxls[pxls[0]],
					 B = edge_pxls[pxls[1]],
					 C = edge_pxls[pxls[2]],
					 D = edge_pxls[pxls[3]];
					 
		cout << A.x << ", " << A.y << ", " << A.z << endl;
		cout << B.x << ", " << B.y << ", " << B.z << endl;
		cout << C.x << ", " << C.y << ", " << C.z << endl;
		cout << D.x << ", " << D.y << ", " << D.z << endl << endl;
		
		
		M_11 = A.x * ((B.y * C.z * 1) + (B.z * 1 * D.y) + (1 * C.y * D.z) - (B.y * 1 * D.z) - (B.z * C.y * 1) - (1 * C.z * D.y)) -
			   A.y * ((B.x * C.z * 1) + (B.z * 1 * D.x) + (1 * C.x * D.z) - (B.x * 1 * D.z) - (B.z * C.x * 1) - (1 * C.z * D.x)) +
			   A.z * ((B.x * C.y * 1) + (B.y * 1 * D.x) + (1 * C.x * D.y) - (B.x * 1 * D.y) - (B.y * C.x * 1) - (1 * C.y * D.x)) -
			   1 * ((B.x * C.y * D.z) + (B.y * C.z * D.x) + (B.z * C.x * D.y) - (B.x * C.z * D.y) - (B.y * C.x * D.z) - (B.z * C.y * D.x));
		
		//cout << M_11 << endl;
		
		if (M_11 != 0) 
		{
			//cout << "WHAT!?" << endl;
			fit_points_4d(DATA_POINTS,pxls, M_12, M_13, M_14, M_15);
			
			sphere.x = 0.5 * (M_12 / M_11);
			sphere.y = -0.5 * (M_13 / M_11);
			sphere.z = 0.5 * (M_14 / M_11);
			sphere.radius = sqrtf(sphere.x * sphere.x + sphere.y * sphere.y + sphere.z * sphere.z  - (M_15 / M_11));

			// put them in the accumulator
			if (in_range((int)sphere.x,0,accum.center_x_y.cols-1) && in_range((int)sphere.y,0,accum.center_x_y.rows-1)
			   && in_range((int)sphere.z,0,accum.center_z_radius.cols-1) && in_range((int)sphere.radius,0,accum.center_z_radius.rows-1))
			{
				//cout << "Ok" << endl;
				accum.center_x_y.cells[(int)sphere.y][(int)sphere.x]++;
				accum.center_z_radius.cells[(int)sphere.radius][(int)sphere.z]++;
			}
		}
		evidence++;
	}
}

// fit a set of points to the general quadratic:
//  Ax^2 + Bxy + Cy^2 + Dx + Ey + F = 0 where C == 0 and A == B
void fit_points_4d(int pxls_cnt, int* pxls, double &M_12, double &M_13, double &M_14, double &M_15) {
	edge_pixel_t A = edge_pxls[pxls[0]],
				 B = edge_pxls[pxls[1]],
				 C = edge_pxls[pxls[2]],
				 D = edge_pxls[pxls[3]];
				 
	double S_1 = A.x*A.x + A.y*A.y + A.z*A.z,
		   S_2 = B.x*B.x + B.y*B.y + B.z*B.z,
		   S_3 = C.x*C.x + C.y*C.y + C.z*C.z,
		   S_4 = D.x*D.x + D.y*D.y + D.z*D.z;
	
	M_12 = S_1 * ((B.y * C.z * 1) + (B.z * 1 * D.y) + (1 * C.y * D.z) - (B.y * 1 * D.z) - (B.z * C.y * 1) - (1 * C.z * D.y)) -
		   A.y * ((S_2 * C.z * 1) + (B.z * 1 * S_4) + (1 * S_3 * D.z) - (S_2 * 1 * D.z) - (B.z * S_3 * 1) - (1 * C.z * S_4)) +
		   A.z * ((S_2 * C.y * 1) + (B.y * 1 * S_4) + (1 * S_3 * D.y) - (S_2 * 1 * D.y) - (B.y * S_3 * 1) - (1 * C.y * S_4)) -
		   1 * ((S_2 * C.y * D.z) + (B.y * C.z * S_4) + (B.z * S_3 * D.y) - (S_2 * C.z * D.y) - (B.y * S_3 * D.z) - (B.z * C.y * S_4));
		   
	M_13 = S_1 * ((B.x * C.z * 1) + (B.z * 1 * D.x) + (1 * C.x * D.z) - (B.x * 1 * D.z) - (B.z * C.x * 1) - (1 * C.z * D.x)) -
		   A.x * ((S_2 * C.z * 1) + (B.z * 1 * S_4) + (1 * S_3 * D.z) - (S_2 * 1 * D.z) - (B.z * S_3 * 1) - (1 * C.z * S_4)) +
		   A.z * ((S_2 * C.x * 1) + (B.x * 1 * S_4) + (1 * S_3 * D.x) - (S_2 * 1 * D.x) - (B.x * S_3 * 1) - (1 * C.x * S_4)) -
		   1 * ((S_2 * C.x * D.z) + (B.x * C.z * S_4) + (B.z * S_3 * D.x) - (S_2 * C.z * D.x) - (B.x * S_3 * D.z) - (B.z * C.x * S_4));
	   
	M_14 = S_1 * ((B.x * C.y * 1) + (B.y * 1 * D.x) + (1 * C.x * D.y) - (B.x * 1 * D.y) - (B.y * C.x * 1) - (1 * C.y * D.x)) -
		   A.x * ((S_2 * C.y * 1) + (B.y * 1 * S_4) + (1 * S_3 * D.y) - (S_2 * 1 * D.y) - (B.y * S_3 * 1) - (1 * C.y * S_4)) +
		   A.y * ((S_2 * C.x * 1) + (B.x * 1 * S_4) + (1 * S_3 * D.x) - (S_2 * 1 * D.x) - (B.x * S_3 * 1) - (1 * C.x * S_4)) -
		   1 * ((S_2 * C.x * D.y) + (B.x * C.y * S_4) + (B.y * S_3 * D.x) - (S_2 * C.y * D.x) - (B.x * S_3 * D.y) - (B.y * C.x * S_4));
		   
	M_15 = S_1 * ((B.x * C.y * D.z) + (B.y * C.z * D.x) + (B.z * C.x * D.y) - (B.x * C.z * D.y) - (B.y * C.x * D.z) - (B.z * C.y * D.x)) -
		   A.x * ((S_2 * C.y * D.z) + (B.y * C.z * S_4) + (B.z * S_3 * D.y) - (S_2 * C.z * D.y) - (B.y * S_3 * D.z) - (B.z * C.y * S_4)) +
		   A.y * ((S_2 * C.x * D.z) + (B.x * C.z * S_4) + (B.z * S_3 * D.x) - (S_2 * C.z * D.x) - (B.x * S_3 * D.z) - (B.z * C.x * S_4)) -
		   A.z * ((S_2 * C.x * D.y) + (B.x * C.y * S_4) + (B.y * S_3 * D.x) - (S_2 * C.y * D.x) - (B.x * S_3 * D.y) - (B.y * C.x * S_4));

}

void process_accum(sphere_t *s) {
	int max_i,max_j;

	find_max_2d(&accum.center_x_y,max_i,max_j);
	s->y = max_i;
	s->x = max_j;

	find_max_2d(&accum.center_z_radius,max_i,max_j);
	s->z = max_j;
	s->radius = max_i;

}

void find_max_2d(matrix_t* m, int &max_i, int &max_j) {
	int i,j;

	max_i = 0;
	max_j = 0;
	for (i=0; i < m->rows; i++) {
		for (j=0; j < m->cols; j++) {
			if (m->cells[max_i][max_j] < m->cells[i][j]) {
				max_i = i;
				max_j = j;
			}
		}
	}
}
